#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray, String
import numpy as np
import argparse

def main():
    parser = argparse.ArgumentParser(description='Send pose commands repeatedly.')
    parser.add_argument('--repeat', type=int, default=1,
                        help='Number of times to toggle pose commands')
    args = parser.parse_args()

    rospy.init_node('move_line_commander', anonymous=True)
    pub_joint = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)
    pub_pose = rospy.Publisher('/command_move_line', Float64MultiArray, queue_size=10)

    rospy.sleep(1)

    msg = Float64MultiArray()
    start_joint = [-1.215456, 0.4159984588623047, 0.4808502197265625, -0.7959480285644531, 1.7736701965332031, -0.8081560134887695, -2.131875991821289]

    print("move to start point...")
    msg.data = start_joint
    pub_joint.publish(msg)
    rospy.sleep(5.5)

    for i in range(args.repeat):
        print(f"[{i+1}/{args.repeat}] move line step 1...")
        msg.data = [0.0, -0.380, 0.520, -0.498, -0.499, 0.506, -0.497]
        pub_pose.publish(msg)
        rospy.sleep(2)

        print(f"[{i+1}/{args.repeat}] move line step 2...")
        msg.data = [0.0, -0.380, 0.320, -0.498, -0.499, 0.506, -0.497]
        pub_pose.publish(msg)
        rospy.sleep(2)

        print(f"[{i+1}/{args.repeat}] move line step 3...")
        msg.data = [0.2, -0.380, 0.320, -0.498, -0.499, 0.506, -0.497]
        pub_pose.publish(msg)
        rospy.sleep(2)

        print(f"[{i+1}/{args.repeat}] move line step 4...")
        msg.data = [0.2, -0.380, 0.520, -0.498, -0.499, 0.506, -0.497]
        pub_pose.publish(msg)
        rospy.sleep(2)

if __name__ == '__main__':
    main()


